// #include "mainwindow.h"
// #include <QApplication>
#include "apf.h"

std::vector<NS_APF::Vec2> obstacle_temp = {{3.5, 1.2}, {5, 3.2}, {4, 8}, {7, 5},
                                           {8, 12},    {10, 8},  {4, 13}};

std::vector<NS_APF::Vec2> init_temp = {
    {0, 0},
};

std::vector<NS_APF::Vec2> delta_temp = {
    {0, 0},
    {-1.5, 0},
};

std::vector<NS_APF::Vec2> goal = {
    {18, 18},
};

int main(int argc, char *argv[]) {
  //    QApplication a(argc, argv);
  //    MainWindow w;
  //    w.show()
  NS_APF::APF_Param apf_param;
  float dt = 0.01;
  // apf_param.obstacle_position_ = obstacle_temp;
  apf_param.init_UAV_pos_ = init_temp;
  apf_param.k_a_ = 1;
  apf_param.k_r_ = 0;
  apf_param.k_s_ = 0;
  apf_param.UAV_virtual_mass = 1;
  apf_param.goal_max_distance_ = 100;
  apf_param.detect_radius_ = 1;

  int num = apf_param.init_UAV_pos_.size();

  NS_APF::APF myAPF(&apf_param, NS_APF::Vec2(10, 10), num);

  std::vector<NS_APF::Vec2> position_lists(num);

  while (1) {
    std::vector<NS_APF::Vec2> velocity = myAPF.calculate(position_lists);
    for (int i = 0; i < num; i++) {
      position_lists[i] = position_lists[i] + velocity[i] * dt;
    }
    std::cout << myAPF.UAV_List_[0].attractive_force_ << "-----";
    std::cout << position_lists[0] << std::endl;
  }

  return 0;
  //    return a.exec();
}
